/*
 * mem_storage.c
 *
 *  Created on: Jan 31, 2011
 *      Author: thienlong
 */

#include "mem_storage.h"
#include <stdlib.h>
#include <string.h>
#include <limits.h>
#include "error.h"
#include <stdint.h>
#include <stdio.h>

int align_left(int size, int align);
#define IAL_FREE_PTR(storage)  \
    ((schar*)(storage)->current + (storage)->block_size - (storage)->free_space)

#define IAL_FLOOR_LOG_BASE_2(value, result) { \
	uint32_t v = (value); \
	register unsigned int r = 0; \
	register unsigned int shift = 0; \
	r = (v > 0xFFFF) << 4; \
	v >>= r; \
	shift = (v > 0xFF) << 3; \
	v >>= shift; \
	r |= shift; \
	shift = (v > 0xF) << 2; \
	v >>= shift; \
	r |= shift; \
	shift = (v > 0x3) << 1; \
	v >>= shift; \
	r |= shift; \
	r |= (v >> 1); \
	result = r; \
}

#define IAL_CEIL_LOG_BASE_2(value, result) { \
	uint32_t v = (value) - 1; \
	register unsigned int r; \
	register unsigned int shift; \
	r = (v > 0xFFFF) << 4; \
	v >>= r; \
	shift = (v > 0xFF) << 3; \
	v >>= shift; \
	r |= shift; \
	shift = (v > 0xF) << 2; \
	v >>= shift; \
	r |= shift; \
	shift = (v > 0x3) << 1; \
	v >>= shift; \
	r |= shift; \
	r |= (v >> 1); \
	result = r + 1; \
}

#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
mem_storage* mem_storage_new(int size, bool thread_safe) {
#else
	mem_storage* mem_storage_new(int size) {
#endif
	mem_storage *storage = fast_calloc(sizeof(mem_storage));
	if (size > 0) {
		storage->block_size = size;
	} else {
		storage->block_size = MEM_BLOCK_DEFAULT_SIZE;
	}
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
	if (thread_safe) {
		pthread_mutex_init(&storage->heap_lock, NULL);
		pthread_mutex_init(&storage->stack_lock, NULL);
		storage->thread_safe = true;
	}
#endif
	return storage;
}

mem_block* mem_storage_get_block(mem_storage *storage, int expect_size) {
	int index = 0;
	IAL_CEIL_LOG_BASE_2(expect_size + MEM_BLOCK_HEADER_SIZE, index);
	if (index < MEM_STORAGE_ARRAY_LENGTH) {
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
		if (storage->thread_safe) {
			pthread_mutex_lock(&storage->heap_lock);
			mem_block *block = storage->heap_blocks[index];
			if (block) {
				storage->heap_blocks[index] = block->next;
				pthread_mutex_unlock(&storage->heap_lock);
			} else {
				pthread_mutex_unlock(&storage->heap_lock);
				int size = 1 << index;
				block = malloc(size);
				assert(block);
				block->size = size;
			}
			return block;
		} else {
#endif
			mem_block *block = storage->heap_blocks[index];
			if (block) {
				storage->heap_blocks[index] = block->next;
			} else {
				int size = 1 << index;
				block = malloc(size);
				assert(block);
				block->size = size;
			}
			return block;
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
		}
#endif
	}
	return NULL;
}

void mem_storage_put_block(mem_storage *storage, mem_block *block) {
	int index = 0;
	IAL_FLOOR_LOG_BASE_2(block->size, index);
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
	if (storage->thread_safe) {
		pthread_mutex_lock(&storage->heap_lock);
		block->next = storage->heap_blocks[index];
		storage->heap_blocks[index] = block;
		pthread_mutex_unlock(&storage->heap_lock);
	} else {
#endif
		block->next = storage->heap_blocks[index];
		storage->heap_blocks[index] = block;
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
	}
#endif
}

/* Moves stack pointer to next block.
 If no blocks, allocate new one and link it to the storage: */
static void inext_block(mem_storage *storage) {
	if (!storage)
		AL_Error(AL_StsNullPtr, "null point exception");

	if (!storage->current || !storage->current->next) {
		mem_block *block;

		block = (mem_block *) fast_malloc(storage->block_size);

		/* link block */
		block->next = 0;

		if (storage->top) {
			storage->top->next = block;
			storage->top = block;
		} else {
			storage->top = storage->bottom = storage->current = block;
		}
	}

	if (storage->current->next)
		storage->current = storage->current->next;
	storage->free_space = storage->block_size - MEM_BLOCK_HEADER_SIZE;
	assert( storage->free_space % AL_STRUCT_ALIGN == 0 );
}

void* mem_storage_alloc_fixed_size(mem_storage *storage, int size) {
	schar *ptr = 0;
	if (!storage)
		AL_Error(AL_StsNullPtr, "NULL storage pointer");

	if (size > INT_MAX)
		AL_Error(AL_StsOutOfRange, "Too large memory block is requested");
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
	if (storage->thread_safe) {
		pthread_mutex_lock(&storage->stack_lock);
		assert(storage->free_space % AL_STRUCT_ALIGN == 0);

		if ((size_t) storage->free_space < size) {
			size_t max_free_space = align_left(storage->block_size
					- MEM_BLOCK_HEADER_SIZE, AL_STRUCT_ALIGN);
			if (max_free_space < size)
				AL_Error(AL_StsOutOfRange, "requested size is negative or too big");

			inext_block(storage);
		}

		ptr = IAL_FREE_PTR(storage);
		assert((size_t) ptr % AL_STRUCT_ALIGN == 0);
		storage->free_space = align_left(storage->free_space - (int) size,
				AL_STRUCT_ALIGN);
		pthread_mutex_unlock(&storage->stack_lock);
		return ptr;
	} else {
#endif
		assert(storage->free_space % AL_STRUCT_ALIGN == 0);

		if ((size_t) storage->free_space < size) {
			size_t max_free_space = align_left(storage->block_size
					- MEM_BLOCK_HEADER_SIZE, AL_STRUCT_ALIGN);
			if (max_free_space < size)
				AL_Error(AL_StsOutOfRange, "requested size is negative or too big");

			inext_block(storage);
		}

		ptr = IAL_FREE_PTR(storage);
		assert((size_t) ptr % AL_STRUCT_ALIGN == 0);
		storage->free_space = align_left(storage->free_space - (int) size,
				AL_STRUCT_ALIGN);

		return ptr;
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
	}
#endif
}

/* Allocate continuous buffer of the specified size in the storage: */
void* mem_storage_alloc(mem_storage *storage, int expect_size, mem_type type) {
	if (type == STACK_MEM) {
		return mem_storage_alloc_fixed_size(storage, expect_size);
	}
	mem_block *block = mem_storage_get_block(storage, expect_size);
	return MEM_BLOCK_DATA_PTR(block);
}

void mem_storage_free(mem_storage **storage) {
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
	if ((*storage)->thread_safe) {
		pthread_mutex_lock(&((*storage)->heap_lock));
		mem_storage *mem = *storage;
		if (mem == NULL) {
			return;
		}
		*storage = NULL;
		int i = 0;
		mem_block *block = NULL;
		mem_block *next = NULL;
		for (; i < MEM_STORAGE_ARRAY_LENGTH; ++i) {
			block = mem->heap_blocks[i];
			while (block != NULL) {
				next = block->next;
				fast_free(block);
				block = next;
			}
		}
		pthread_mutex_unlock(&mem->heap_lock);
		pthread_mutex_lock(&mem->stack_lock);
		block = mem->bottom;
		while (block != NULL) {
			next = block->next;
			fast_free(block);
			block = next;
		}
		pthread_mutex_unlock(&mem->stack_lock);
		fast_free(mem);
	} else {
#endif
		mem_storage *mem = *storage;
		if (mem == NULL) {
			return;
		}
		*storage = NULL;
		int i = 0;
		mem_block *block = NULL;
		mem_block *next = NULL;
		for (; i < MEM_STORAGE_ARRAY_LENGTH; ++i) {
			block = mem->heap_blocks[i];
			while (block != NULL) {
				next = block->next;
				fast_free(block);
				block = next;
			}
		}
		block = mem->bottom;
		while (block != NULL) {
			next = block->next;
			fast_free(block);
			block = next;
		}
		fast_free(mem);
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
	}
#endif
}

void mem_storage_clear(mem_storage *storage) {
	if (!storage)
		AL_Error(AL_StsNullPtr, "null point exception");
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
	if (storage->thread_safe) {
		pthread_mutex_lock(&storage->stack_lock);
		storage->top = storage->bottom;
		storage->free_space = storage->bottom ? storage->block_size
				- MEM_BLOCK_HEADER_SIZE : 0;
		pthread_mutex_unlock(&storage->stack_lock);
	} else {
#endif
		storage->top = storage->bottom;
		storage->free_space = storage->bottom ? storage->block_size
				- MEM_BLOCK_HEADER_SIZE : 0;
#ifdef MEM_STORAGE_SUPPORT_THREAD_SAFE
	}
#endif
}

//int ceilbase2(int value) {
//	int counter = 1;
//	while ((value = value >> 1) != 0) {
//		++counter;
//	}
//	return 1 << counter;
//}

#define alignPtr(ptr, align) (((size_t)(ptr) + (align)-1) & -(align))

int align(int size, int align) {
	assert((align & (align - 1)) == 0 && size < INT_MAX);
	return (size + align - 1) & -align;
}

int align_left(int size, int align) {
	return size & -align;
}

void* fast_malloc(size_t size) {
	uchar* udata = (uchar*) malloc(size + sizeof(void*) + AL_MALLOC_ALIGN);
	if (!udata)
		return NULL;
	uchar** adata = (uchar**)alignPtr((uchar**)udata + 1, AL_MALLOC_ALIGN);
	adata[-1] = udata;
	return adata;
}

void* fast_calloc(size_t size) {
	uchar* udata = (uchar*) calloc(1, size + sizeof(void*) + AL_MALLOC_ALIGN);
	if (!udata)
		return NULL;
	uchar** adata = (uchar**)alignPtr((uchar**)udata + 1, AL_MALLOC_ALIGN);
	adata[-1] = udata;
	return adata;
}

void fast_free(void* ptr) {
	if (ptr) {
		uchar* udata = ((uchar**) ptr)[-1];
		AL_DbgAssert(udata < (uchar*) ptr && ((uchar*) ptr - udata)
				<= (ptrdiff_t) (sizeof(void*) + AL_MALLOC_ALIGN));
		free(udata);
	}
}

void mem_free(void *ptr, mem_storage *storage, mem_type type) {
	if (!storage)
		free(ptr);
	else if (type == HEAP_MEM) {
		mem_storage_put_block(storage, MEM_BLOCK_PTR(ptr));
	} else {
		assert(type == STACK_MEM);
	}
}

